/*
 * Copyright (c) 2011-2013 Jarek Sacha. All Rights Reserved.
 *
 * Author's e-mail: jpsacha at gmail.com
 */
package samples;

import br.com.manchini.stereocv.utils.MatcherUtils;
import br.com.manchini.stereocv.utils.RobustMatcher;
import com.googlecode.javacv.CanvasFrame;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import static com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_features2d;
import static com.googlecode.javacv.cpp.opencv_highgui.*;
import static com.googlecode.javacv.cpp.opencv_features2d.*;
import com.googlecode.javacv.cpp.opencv_imgproc;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_RGB2GRAY;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvCvtColor;
import com.googlecode.javacv.cpp.opencv_nonfree;
import java.util.ArrayList;

/**
 * The example for section "Matching images using random sample consensus" in
 * Chapter 9, page 233.
 *
 * Most of the computations are done by `RobustMatcher` helper class.
 */
public final class Ex4MatchingUsingSampleConsensus {

    public DMatch toNativeVector(ArrayList<DMatch> src) {
        DMatch dest = new DMatch(src.size());
        for (int i = 0; i < src.size(); i++) {
            // Since there is no way to `put` objects into a vector DMatch,
            // We have to reassign all values individually, and hope that API will not any new ones.
            copy(src.get(i), dest.position(i));
        }
        // Set position to 0 explicitly to avoid issues from other uses of this position-based container.
        dest.position(0);

        return dest;
    }

    void copy(DMatch src, DMatch dest) {
        dest.put(src.limit(src.position() + 1));
    }

    public Ex4MatchingUsingSampleConsensus() {

        // Read input images
        IplImage image1 = cvLoadImage(("img/6esq.jpg"));
        IplImage image2 = cvLoadImage(("img/6dir.jpg"));
//    cvshow(image1, "Right image")
//    show(image2, "Left image")

        // Prepare the matcher
        RobustMatcher rMatcher = new RobustMatcher(
                0.98,
                1.0,
                0.65F,
                new opencv_nonfree.SURF(10),
                true);

        //
        // Match two images
        //


        // draw the matches
        IplImage matchesCanvas = IplImage.create(new CvSize(image1.width() + image2.width(), image1.height()), image1.depth(), 3);

//    show(matchesCanvas, "Matches")


        RobustMatcher.Result matches = rMatcher.matchImages(image1, image2);
        matches.getMatches();



        // Draw the epipolar lines
        CvPoint2D32f[] points = MatcherUtils.toCvPoint2D32f(matches.getMatches(), matches.getKeyPoints1(), matches.getKeyPoints2());
        CvPoint2D32f point1 = points[0];
        CvPoint2D32f point2 = points[1];

        System.out.println("Ponto Referente");
//        System.out.println("Esq: "+point1.position(10).x()+" x "+point1.position(10).y());
//        System.out.println("Dir: "+point2.position(10).x()+" x "+point2.position(10).y());
        int indexPontoMaisProximo = MatcherUtils.getIndexPontoMaisProximo(image1.width() / 2, image1.height() / 2, point2);

        System.out.println("Esq: " + point1.position(indexPontoMaisProximo).x() + " x " + point1.position(indexPontoMaisProximo).y());
        System.out.println("Dir: " + point2.position(indexPontoMaisProximo).x() + " x " + point2.position(indexPontoMaisProximo).y());
        System.out.println("Distancia: "+matches.getMatches().get(indexPontoMaisProximo).distance());

//        KeyPoint keyPoint1 = new KeyPoint().pt(point1.position(indexPontoMaisProximo));
//        
//        KeyPoint keyPoint2 = new KeyPoint().pt(point2.position(indexPontoMaisProximo));

        int xp = new Long(Math.round(point1.position(indexPontoMaisProximo).x())).intValue();
        int yp = new Long(Math.round(point1.position(indexPontoMaisProximo).y())).intValue();
        cvCircle(image1, new CvPoint(xp, yp), 3, CvScalar.RED, 1, CV_AA, 0);
        Canv